#include "motor_foc.h"
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include "stm32f1xx_hal.h"

const MotorParamType DEFAULT_MOTOR_PARAMS = {
    .phase_resistance = 0.1f,    // 0.1欧姆	相位阻抗
    .phase_inductance = 0.0001f, // 0.1mH	相位电感值
    .back_emf_constant = 0.05f,  // V/rad/s	反电动势常数
    .poles = 14,                 // 7对极 极对数
    .max_current = 15000,         // 15A (Q12格式)	电流最大值
    .max_voltage = 24000,         // 24V (Q12格式)	电压最大值
    .max_rpm = 10000              // 10000RPM (Q12格式) 旋转速度最大值
};

const uint8_t default_hall_order[6] = {1, 3, 2, 6, 4, 5};
static FOC_LogEntry_t g_foc_log_buffer[FOC_LOG_BUFFER_SIZE];
static uint16_t g_foc_log_index = 0;


static void pi_init(PIController* pi, int32_t kp, int32_t ki, int32_t min, int32_t max);
static void calculate_pi_params(const MotorParamType* motor_params, 
                               int32_t* kp_current, int32_t* ki_current,
                               int32_t* kp_speed, int32_t* ki_speed);

static uint8_t FOC_CheckTemperature(FOC_Type* controller);
static uint8_t FOC_CheckCurrentLimit(FOC_Type* controller);
// 正弦查找表（Q12格式）
static const int16_t sin_table[] = {
    0, 402, 803, 1205, 1606, 2006, 2404, 2801, 3196, 3589,
    3981, 4369, 4756, 5139, 5519, 5896, 6269, 6639, 7005, 7366,
    7723, 8076, 8423, 8765, 9102, 9434, 9760, 10080, 10394, 10702,
    11003, 11297, 11585, 11866, 12140, 12406, 12665, 12916, 13160, 13395,
    13623, 13842, 14053, 14256, 14449, 14635, 14811, 14978, 15137, 15286,
    15426, 15557, 15679, 15791, 15893, 15986, 16069, 16143, 16206, 16260,
    16305, 16339, 16364, 16379, 16384
};

// 获取正弦值（Q12格式）
static int16_t sin_fixed(int16_t angle) {
    angle = angle % 16384;
    if (angle < 0) angle += 16384;
    
    // 使用线性插值提高精度
    int index = angle >> 7; // 除以128
    int remainder = angle & 0x7F; // 取余数
    
    if (index >= 128) index = 0; // 边界检查
    
    int16_t val1 = sin_table[index];
    int16_t val2 = sin_table[index + 1];
    
    // 线性插值
    return val1 + ((int32_t)(val2 - val1) * remainder) / 128;
}

// 获取余弦值（Q12格式）
static int16_t cos_fixed(int16_t angle) {
    return sin_fixed(angle + 4096); // cos(θ) = sin(θ + 90°)
}

// FOC初始化
void FOC_Init(FOC_Type* controller) {
    // 参数验证
    if (controller == NULL) {
        return;
    }
    
    // 初始化默认参数
    memset(controller, 0, sizeof(FOC_Type)); // 清零整个结构体
    
    controller->param.z_ctrlTypSel = Motor_Ctrl_FOC;
    controller->param.b_fieldWeakEna = 0;
    controller->param.id_fieldWeakMax = 4000;
    controller->param.a_phaAdvMax = 400;
    controller->param.r_fieldWeakHi = 16000;
    controller->param.r_fieldWeakLo = 12000;
    controller->param.i_max = 12000;
    controller->param.n_max = 16000;
    controller->param.b_angleMeasEna = 0;
    controller->param.z_selPhaCurMeasABC = 0;
    controller->param.b_diagEna = 1;
    controller->param.bat_overvoltage_threshold = BAT_OVERVOLTAGE_THRESHOLD;
    controller->param.bat_undervoltage_threshold = BAT_UNDERVOLTAGE_THRESHOLD;
    controller->param.temp_overtemp_threshold = TEMP_OVERTEMP_THRESHOLD;
    
    // 使用默认电机参数
    controller->motor_param = DEFAULT_MOTOR_PARAMS;
    
    // 关键参数有效性检查
    if (controller->motor_param.phase_resistance <= 0 || 
        controller->motor_param.phase_inductance <= 0 || 
        controller->motor_param.back_emf_constant <= 0 ||
        controller->motor_param.poles <= 0 ||
        controller->motor_param.max_current <= 0 ||
        controller->motor_param.max_voltage <= 0 ||
        controller->motor_param.max_rpm <= 0) {
        
        controller->error_code = FOC_Err_Motor_Param_Invalid;
        controller->outputs.z_errCode = FOC_Err_Motor_Param_Invalid;
        FOC_LogEvent(FOC_LOG_EVENT_FAULT, FOC_Err_Motor_Param_Invalid);
        return; // 拒绝运行
    }
    
    // 初始化输入
    controller->inputs.b_motEna = 0;
    controller->inputs.z_ctrlModReq = Motor_FOC_Mode_OPEN;
    controller->inputs.r_inpTgt = 0;
    controller->inputs.b_hallA = 0;
    controller->inputs.b_hallB = 0;
    controller->inputs.b_hallC = 0;
    controller->inputs.i_phaAB = 0;
    controller->inputs.i_phaBC = 0;
    controller->inputs.i_DCLink = 0;
    controller->inputs.a_mechAngle = 0;
    
    // 初始化输出
    controller->outputs.DC_phaA = 0;
    controller->outputs.DC_phaB = 0;
    controller->outputs.DC_phaC = 0;
    controller->outputs.n_mot = 0;
    controller->outputs.a_elecAngle = 0;
    controller->outputs.z_errCode = FOC_Err_None;
    
    // 初始化内部状态
    for (int i = 0; i < 10; i++) {
        controller->internalStates[i] = 0;
    }

    // 初始化缓启动相关变量
    controller->softStartStatus = SOFT_START_NOT_STARTED;
    controller->softStartAttempts = 0;
    controller->softStartTime = 0;
    
    // 初始化运行时检测相关变量
    controller->hallErrorCount = 0;
    controller->currentErrorCount = 0;
    controller->lastHallState = 0;
    
    // 根据电机参数计算PI控制器参数
    int32_t kp_current, ki_current, kp_speed, ki_speed;
    calculate_pi_params(&controller->motor_param, &kp_current, &ki_current, &kp_speed, &ki_speed);
    
    // 初始化d轴电流PI控制器
    pi_init(&controller->id_controller, 
            kp_current, 
            ki_current, 
            -controller->param.i_max, 
            controller->param.i_max);
    
    // 初始化q轴电流PI控制器
    pi_init(&controller->iq_controller, 
            kp_current, 
            ki_current, 
            -controller->param.i_max, 
            controller->param.i_max);
    
    // 初始化速度环PI控制器
    pi_init(&controller->speed_controller, 
            kp_speed, 
            ki_speed, 
            -controller->param.n_max, 
            controller->param.n_max);

    // 初始化速度估算参数
    controller->speed_params.filter_alpha = float_to_fixed(0.1f);
    controller->speed_params.min_sample_time = 100; // 100us最小采样时间
    controller->speed_params.max_angle_diff = 4096; // 90度
    controller->speed_params.poles = controller->motor_param.poles;
    
    // 初始化速度估算状态
    controller->speed_filtered = 0;
    controller->speed_accumulator = 0;
    controller->prev_elec_angle = 0;
    controller->last_update_time = HAL_GetTick();
    
    // 初始化角度累积器
    controller->angle_accumulator = 0;
    controller->angle_accumulator_prev = 0;
    
    // 初始化错误状态
    controller->error_code = FOC_Err_None;
    for (int i = 0; i < FOC_Err_MAX_ERRORS; i++) {
        controller->error_counters[i] = 0;
    }
    
    // 初始化霍尔校准参数（默认值）
    for (int i = 0; i < 8; i++) {
        controller->hall_params.hall_offset[i] = 0;
    }
    controller->hall_params.hall_phase_error = 0;
    
    // 默认霍尔顺序（根据实际电机调整）
    memcpy(controller->hall_params.hall_order, default_hall_order, 6);
    
    // 初始化电流校准参数
    controller->current_calib.dc_link_offset = 0;
    controller->current_calib.dc_link_scale = 1.0f;
    controller->current_calib.is_calibrated = 0;
    // 初始化滤波系数
    controller->speed_filter_coef = float_to_fixed(0.1f);
}

// 改进的速度估算函数
static int32_t estimate_speed(FOC_Type* controller, int16_t elec_angle, uint32_t current_time) {
    if (controller == NULL) {
        return 0;
    }
    
    // 计算时间差（考虑计数器溢出）
    uint32_t time_diff;
    if (current_time >= controller->last_update_time) {
        time_diff = current_time - controller->last_update_time;
    } else {
        // 处理计数器溢出
        time_diff = (UINT32_MAX - controller->last_update_time) + current_time + 1;
    }
    
    // 添加异常值检查
    if (time_diff > 1000000) { // 超过1秒认为异常
        return controller->outputs.n_mot;
    }

    if (time_diff < controller->speed_params.min_sample_time) {
        return controller->outputs.n_mot; // 返回上一次的速度值
    }
    
    // 计算角度差（处理角度环绕）
    int32_t angle_diff = elec_angle - controller->prev_elec_angle;
    if (angle_diff > 8192) { // 处理正向环绕
        angle_diff -= 16384;
    } else if (angle_diff < -8192) { // 处理负向环绕
        angle_diff += 16384;
    }
    
    // 检查角度差是否合理，防止异常值
    if (abs(angle_diff) > controller->speed_params.max_angle_diff) {
        return controller->outputs.n_mot; // 返回上一次的速度值
    }
    
    // 计算原始速度（考虑采样时间）
    // 转速(rpm) = (角度差 / 360°) * (60 * 10^6) / (时间差 * 极对数)
    int32_t raw_speed = (angle_diff * 937500) / (time_diff * controller->speed_params.poles);
    
    // 使用一阶低通滤波器
    int32_t filtered_speed = controller->speed_filtered;
    int32_t error = raw_speed - filtered_speed;
    
    // 应用滤波
    filtered_speed += (error * controller->speed_params.filter_alpha) >> FIXED_SHIFT;
    
    // 更新状态
    controller->prev_elec_angle = elec_angle;
    controller->last_update_time = current_time;
    controller->speed_filtered = filtered_speed;
    
    return filtered_speed;
}

// 根据电机参数计算PI控制器参数
static void calculate_pi_params(const MotorParamType* motor_params, 
                               int32_t* kp_current, int32_t* ki_current,
                               int32_t* kp_speed, int32_t* ki_speed) {
    // 电流环带宽（rad/s）
    float bandwidth_current = 2 * PI * 1000; // 1kHz带宽
    
    // 电流环PI参数计算
    *kp_current = float_to_fixed(bandwidth_current * motor_params->phase_inductance);
    *ki_current = float_to_fixed(bandwidth_current * motor_params->phase_resistance);
    
    // 速度环带宽（rad/s）
    float bandwidth_speed = 2 * PI * 100; // 100Hz带宽
    
    // 计算机械时间常数
    float j = 0.001f; // 转动惯量（假设值，需要根据实际电机调整）
    float b = 0.0001f; // 阻尼系数（假设值）
    
    // 速度环PI参数计算
    *kp_speed = float_to_fixed(2 * bandwidth_speed * j);
    *ki_speed = float_to_fixed(bandwidth_speed * bandwidth_speed * j);
}


// 修改 FOC_FaultDetection 函数，在函数开始处添加参数有效性检查：
uint8_t FOC_FaultDetection(FOC_Type* controller, int16_t bat_voltage, int16_t temperature) {
    if (controller == NULL) {
        return 1; // 检测到故障
    }
    
    uint8_t fault_detected = 0;
    
    // 使用参数中的阈值而不是硬编码的宏
    uint16_t overvoltage_threshold = controller->param.bat_overvoltage_threshold;
    uint16_t undervoltage_threshold = controller->param.bat_undervoltage_threshold;
    uint16_t overtemp_threshold = controller->param.temp_overtemp_threshold;
    
    // 1. 过流检测 (包括DC链路电流)
    if (abs(controller->inputs.i_DCLink) > controller->param.i_max * 3 / 2) {
        controller->error_counters[FOC_Err_Motor_OverCurrent]++;
        if (controller->error_counters[FOC_Err_Motor_OverCurrent] > 5) {
            controller->error_code = FOC_Err_Motor_OverCurrent;
            fault_detected = 1;
        }
    } else {
        controller->error_counters[FOC_Err_Motor_OverCurrent] = 0;
    }
    
    // 2. 过压检测
    if (bat_voltage > overvoltage_threshold) {
        controller->error_counters[FOC_Err_Motor_OverVoltage]++;
        if (controller->error_counters[FOC_Err_Motor_OverVoltage] > 5) {
            controller->error_code = FOC_Err_Motor_OverVoltage;
            fault_detected = 1;
        }
    } else {
        controller->error_counters[FOC_Err_Motor_OverVoltage] = 0;
    }
    
    // 3. 欠压检测
    if (bat_voltage < undervoltage_threshold) {
        controller->error_counters[FOC_Err_Motor_UnderVoltage]++;
        if (controller->error_counters[FOC_Err_Motor_UnderVoltage] > 5) {
            controller->error_code = FOC_Err_Motor_UnderVoltage;
            fault_detected = 1;
        }
    } else {
        controller->error_counters[FOC_Err_Motor_UnderVoltage] = 0;
    }
    
    // 4. 过温检测 (使用传入的温度参数)
    if (temperature > overtemp_threshold) {
        controller->error_counters[FOC_Err_Motor_OverTemperature]++;
        if (controller->error_counters[FOC_Err_Motor_OverTemperature] > 5) {
            controller->error_code = FOC_Err_Motor_OverTemperature;
            fault_detected = 1;
        }
    } else {
        controller->error_counters[FOC_Err_Motor_OverTemperature] = 0;
    }
    
    // 5. 霍尔传感器故障检测
    if (controller->softStartStatus == SOFT_START_SUCCESS && 
        abs(controller->outputs.n_mot) > 50) {
        uint8_t currentHallState = (controller->inputs.b_hallA << 2) | 
                                  (controller->inputs.b_hallB << 1) | 
                                  controller->inputs.b_hallC;
        
        if (currentHallState == controller->lastHallState) {
            controller->error_counters[FOC_Err_Motor_Hall_Error]++;
            if (controller->error_counters[FOC_Err_Motor_Hall_Error] > 10) {
                controller->error_code = FOC_Err_Motor_Hall_Error;
                fault_detected = 1;
            }
        } else {
            controller->error_counters[FOC_Err_Motor_Hall_Error] = 0;
            controller->lastHallState = currentHallState;
        }
    }
    
    // 6. 电流传感器故障检测
    int16_t i_a = controller->inputs.i_phaAB;
    int16_t i_b = controller->inputs.i_phaBC;
    int16_t i_c = -i_a - i_b; // 假设三相电流和为0
    
    if (abs(i_a) > controller->param.i_max || 
        abs(i_b) > controller->param.i_max || 
        abs(i_c) > controller->param.i_max) {
        controller->error_counters[FOC_Err_Motor_Curr_Sensor_Failed]++;
        if (controller->error_counters[FOC_Err_Motor_Curr_Sensor_Failed] > 5) {
            controller->error_code = FOC_Err_Motor_Curr_Sensor_Failed;
            fault_detected = 1;
        }
    } else {
        controller->error_counters[FOC_Err_Motor_Curr_Sensor_Failed] = 0;
    }
    
    // 7. 检查电机参数有效性
    if (controller->motor_param.phase_resistance <= 0 || 
        controller->motor_param.phase_inductance <= 0 || 
        controller->motor_param.poles <= 0) {
        controller->error_code = FOC_Err_Motor_Param_Invalid;
        fault_detected = 1;
    }
    
    // 设置错误代码到输出
    if (fault_detected) {
        controller->outputs.z_errCode = controller->error_code;
    } else {
        controller->outputs.z_errCode = FOC_Err_None;
    }
    
    return fault_detected;
}

// 改进的霍尔角度计算函数
static int16_t calculate_hall_angle(FOC_Type* controller) {
    uint8_t hall_state = (controller->inputs.b_hallA << 2) | 
                        (controller->inputs.b_hallB << 1) | 
                        controller->inputs.b_hallC;
    
    // 基本霍尔到角度映射（60度间隔）
    const int16_t base_angles[] = {0, 2731, 5461, 8192, 10923, 13653};
    
    // 应用偏移量和相位修正
    int16_t angle = base_angles[hall_state] + 
                   controller->hall_params.hall_offset[hall_state] +
                   controller->hall_params.hall_phase_error;
    
    // 处理角度环绕
    if (angle < 0) angle += 16384;
    if (angle >= 16384) angle -= 16384;
    
    return angle;
}

// 霍尔传感器校准函数
uint8_t FOC_CalibrateHall(FOC_Type* controller, const HallCalibParamType* params) {
    // 参数验证
    if (params == NULL) {
        return 0; // 失败
    }
    
    // 检查霍尔偏移量是否在合理范围内（±60度电气角度）
    for (int i = 0; i < 8; i++) {
        if (abs(params->hall_offset[i]) > 2731) { // 60°电气角度 ≈ 2731 (16384/6)
            controller->outputs.z_errCode = FOC_Err_Motor_Hall_Calib_Error;
            return 0; // 失败
        }
    }
    
    // 检查相位误差是否在合理范围内
    if (abs(params->hall_phase_error) > 1365) { // ±30度
        controller->outputs.z_errCode = FOC_Err_Motor_Hall_Calib_Error;
        return 0; // 失败
    }
    
    // 检查霍尔顺序是否有效且构成完整排列
    uint8_t valid = 1;
    uint8_t used[7] = {0}; // 索引0-6，用于标记1-6是否已被使用
    
    for (int i = 0; i < 6; i++) {
        // 检查值是否在有效范围内
        if (params->hall_order[i] < 1 || params->hall_order[i] > 6) {
            valid = 0;
            break;
        }
        
        // 检查是否重复
        if (used[params->hall_order[i]] != 0) {
            valid = 0; // 发现重复值
            break;
        }
        
        // 标记该值已被使用
        used[params->hall_order[i]] = 1;
    }
    
    // 检查是否包含所有1-6的值
    for (int i = 1; i <= 6; i++) {
        if (used[i] == 0) {
            valid = 0; // 缺少某个值
            break;
        }
    }
    
    if (!valid) {
        controller->outputs.z_errCode = FOC_Err_Motor_Hall_Calib_Error;
        return 0; // 失败
    }
    
    // 应用校准参数
    controller->hall_params = *params;
    
    // 重置霍尔相关状态
    controller->hallErrorCount = 0;
    controller->lastHallState = 0;
    
    return 1; // 成功
}

// 设置FOC参数
void FOC_SetParameters(FOC_Type* controller, const FOC_Param_Type* param) {
    controller->param = *param;
}

// 设置FOC输入
void FOC_SetInputs(FOC_Type* controller, const FOCInputs* inputs) {
    controller->inputs = *inputs;
}

// 获取FOC输出
void FOC_GetOutputs(FOC_Type* controller, FOCOutputs* outputs) {
    *outputs = controller->outputs;
}

// Clarke变换 (ABC -> αβ)
static void clarke_transform(int16_t a, int16_t b, int16_t c, int16_t* alpha, int16_t* beta) {
    // Clarke变换公式:
    // i_α = (2/3) * (i_a - 0.5*i_b - 0.5*i_c)
    // i_β = (1/√3) * (i_b - i_c)
    
    int32_t i_alpha = (2 * a - b - c) / 3;
    int32_t i_beta = (b - c) * ONE_BY_SQRT3 * FIXED_SCALE;
    
    *alpha = (int16_t)i_alpha;
    *beta = (int16_t)(i_beta >> FIXED_SHIFT);
}

// Park变换 (αβ -> dq)
static void park_transform(int16_t alpha, int16_t beta, int16_t angle, int16_t* d, int16_t* q) {
    int16_t cos_theta = cos_fixed(angle);
    int16_t sin_theta = sin_fixed(angle);
    
    // Park变换公式:
    // i_d = i_α * cos(θ) + i_β * sin(θ)
    // i_q = -i_α * sin(θ) + i_β * cos(θ)
    
    int32_t i_d = (alpha * cos_theta + beta * sin_theta) >> FIXED_SHIFT;
    int32_t i_q = (-alpha * sin_theta + beta * cos_theta) >> FIXED_SHIFT;
    
    *d = (int16_t)i_d;
    *q = (int16_t)i_q;
}

// 逆Park变换 (dq -> αβ)
static void inverse_park_transform(int16_t d, int16_t q, int16_t angle, int16_t* alpha, int16_t* beta) {
    int16_t cos_theta = cos_fixed(angle);
    int16_t sin_theta = sin_fixed(angle);
    
    // 逆Park变换公式:
    // u_α = u_d * cos(θ) - u_q * sin(θ)
    // u_β = u_d * sin(θ) + u_q * cos(θ)
    
    int32_t u_alpha = (d * cos_theta - q * sin_theta) >> FIXED_SHIFT;
    int32_t u_beta = (d * sin_theta + q * cos_theta) >> FIXED_SHIFT;
    
    *alpha = (int16_t)u_alpha;
    *beta = (int16_t)u_beta;
}

// 空间矢量调制 (SVM)
static void space_vector_modulation(int16_t alpha, int16_t beta, int16_t* a, int16_t* b, int16_t* c) {
    // 计算三相占空比
    int32_t u_a = alpha;
    int32_t u_b = (-alpha + SQRT3 * beta) / 2;
    int32_t u_c = (-alpha - SQRT3 * beta) / 2;
    
    // 计算偏移量以实现中心对齐
    int32_t offset = (u_a + u_b + u_c) / 3;
    
    u_a -= offset;
    u_b -= offset;
    u_c -= offset;
    
    // 限制在有效范围内
    u_a = (u_a > FIXED_SCALE) ? FIXED_SCALE : (u_a < -FIXED_SCALE) ? -FIXED_SCALE : u_a;
    u_b = (u_b > FIXED_SCALE) ? FIXED_SCALE : (u_b < -FIXED_SCALE) ? -FIXED_SCALE : u_b;
    u_c = (u_c > FIXED_SCALE) ? FIXED_SCALE : (u_c < -FIXED_SCALE) ? -FIXED_SCALE : u_c;
    
    *a = (int16_t)u_a;
    *b = (int16_t)u_b;
    *c = (int16_t)u_c;
}

// PI控制器初始化
static void pi_init(PIController* pi, int32_t kp, int32_t ki, int32_t min, int32_t max) {
    pi->kp = kp;
    pi->ki = ki;
    pi->integral = 0;
    pi->min_output = min;
    pi->max_output = max;
}

// PI控制器计算
static int32_t pi_calculate(PIController* pi, int32_t error) {
    if (pi == NULL) {
        return 0;
    }
    
    // 比例项
    int32_t proportional = (pi->kp * error) >> FIXED_SHIFT;
    
    // 积分项更新（使用clamping抗饱和方法）
    int64_t new_integral = pi->integral + ((int64_t)pi->ki * error);
    
	// 积分限幅，防止积分饱和
    int64_t integral_limit = (int64_t)pi->max_output << FIXED_SHIFT;
    if (new_integral > integral_limit) {
        new_integral = integral_limit;
    } else if (new_integral < -integral_limit) {
        new_integral = -integral_limit;
    }

    int32_t integral_output = (int32_t)(new_integral >> FIXED_SHIFT);

    // 计算总输出（比例+积分）
    int32_t output = proportional + integral_output;
    
    // 抗饱和处理（使用clamping方法）
    if (output > pi->max_output) {
        // 正向饱和：限制输出为最大值，并停止积分累加
        output = pi->max_output;
        // 只有当积分项不会使输出进一步增加时才更新积分
        // 即：当误差为正且积分项为正时，不更新积分
        if (!((error > 0) && (integral_output > 0))) {
            pi->integral = new_integral;
        }
    } else if (output < pi->min_output) {
        // 负向饱和：限制输出为最小值，并停止积分累加
        output = pi->min_output;
        // 只有当积分项不会使输出进一步减小时才更新积分
        // 即：当误差为负且积分项为负时，不更新积分
        if (!((error < 0) && (integral_output < 0))) {
            pi->integral = new_integral;
        }
    } else {
        // 未饱和：正常更新积分项
        pi->integral = new_integral;
    }
    
    // 确保积分项不溢出
    if (pi->integral > (INT32_MAX >> 2)) {
        pi->integral = INT32_MAX >> 2;
    } else if (pi->integral < (INT32_MIN >> 2)) {
        pi->integral = INT32_MIN >> 2;
    }
    
    return output;
}

// FOC主循环
void FOC_Step(FOC_Type* controller, int16_t bat_voltage, int16_t temperature) {
    // 参数验证
    if (controller == NULL) {
        return;
    }
    #ifdef USE_WATCHDOG
    HAL_IWDG_Refresh(&hiwdg); // 假设使用独立看门狗
    #endif
    // 如果缓启动尚未完成，尝试缓启动
    if (controller->softStartStatus != SOFT_START_SUCCESS && 
        controller->softStartStatus != SOFT_START_FAILED) {
        uint8_t startResult = FOC_SoftStart(controller);
        if (startResult == 2) { // 启动失败
            // 停止电机输出
            controller->outputs.DC_phaA = 0;
            controller->outputs.DC_phaB = 0;
            controller->outputs.DC_phaC = 0;
            return;
        } else if (startResult == 0) {
            // 启动中，直接返回，保持当前输出
            return;
        }
    }
    
    // 如果缓启动失败，直接返回
    if (controller->softStartStatus == SOFT_START_FAILED) {
        controller->outputs.DC_phaA = 0;
        controller->outputs.DC_phaB = 0;
        controller->outputs.DC_phaC = 0;
        controller->outputs.z_errCode = FOC_Err_Motor_Start_Failed;
        return;
    }
    
    // 检查电机是否使能
    if (!controller->inputs.b_motEna) {
        controller->outputs.DC_phaA = 0;
        controller->outputs.DC_phaB = 0;
        controller->outputs.DC_phaC = 0;
        controller->outputs.z_errCode = FOC_Err_None;
        // 重置缓启动状态
        controller->softStartStatus = SOFT_START_NOT_STARTED;
        return;
    }
    
    // 检查霍尔传感器状态是否有效
    uint8_t current_hall_state = (controller->inputs.b_hallA << 2) | 
                                (controller->inputs.b_hallB << 1) | 
                                controller->inputs.b_hallC;
    
    // 检查是否为无效霍尔状态 (000 或 111)
	if (current_hall_state == 0 || current_hall_state == 7) {
		// 霍尔传感器故障处理
		FOC_SetError(controller, FOC_Err_Motor_Hall_Error);
		
		// 处理方式1: 强制停机
		controller->outputs.DC_phaA = 0;
		controller->outputs.DC_phaB = 0;
		controller->outputs.DC_phaC = 0;
		motor_pwm_set_foc_outputs(g_pwm_channels, &controller->outputs);
		controller->softStartStatus = SOFT_START_FAILED;
		
		// 记录故障日志
		FOC_LogEvent(FOC_LOG_EVENT_FAULT, FOC_Err_Motor_Hall_Error);
		return;

        /* 
        // 处理方式2: 试图重启电机(可选实现)
        // 重置缓启动状态以尝试重新启动
        controller->softStartStatus = SOFT_START_NOT_STARTED;
        controller->outputs.DC_phaA = 0;
        controller->outputs.DC_phaB = 0;
        controller->outputs.DC_phaC = 0;
        motor_pwm_set_foc_outputs(g_pwm_channels, &controller->outputs);
        return;
        */
    }
    
    // 故障检测
    if (FOC_FaultDetection(controller, bat_voltage, temperature)) {
        // 检测到故障，停止电机
        controller->outputs.DC_phaA = 0;
        controller->outputs.DC_phaB = 0;
        controller->outputs.DC_phaC = 0;
        //controller->softStartStatus = SOFT_START_FAILED;
        return;
    }
    if (controller->param.b_autoRecoveryEna && 
    controller->softStartStatus == SOFT_START_FAILED) {
		uint32_t currentTime = HAL_GetTick();
		if (currentTime - controller->faultStartTime > controller->param.autoRecoveryDelay) {
			// 尝试自动恢复
			if (controller->autoRecoveryAttempt < 3) { // 最多尝试3次
				FOC_ClearFaults(controller);
				controller->autoRecoveryAttempt++;
				FOC_LogEvent(FOC_LOG_EVENT_INIT, 0x3001); // 自动恢复尝试
			}
		}
	}
    // 检查温度差异和驱动板温度
    if (FOC_CheckTemperature(controller)) {
        // 检测到温度异常，停止电机
        controller->outputs.DC_phaA = 0;
        controller->outputs.DC_phaB = 0;
        controller->outputs.DC_phaC = 0;
        motor_pwm_set_foc_outputs(g_pwm_channels, &controller->outputs);
        controller->softStartStatus = SOFT_START_FAILED;
        return;
    }
    
    // 检查驱动板电流限制
    if (FOC_CheckCurrentLimit(controller)) {
        // 检测到电流异常，停止电机
        controller->outputs.DC_phaA = 0;
        controller->outputs.DC_phaB = 0;
        controller->outputs.DC_phaC = 0;
        motor_pwm_set_foc_outputs(g_pwm_channels, &controller->outputs);
        controller->softStartStatus = SOFT_START_FAILED;
        return;
    }

    // 获取电角度
    int16_t elec_angle;
    if (controller->param.b_angleMeasEna) {
        elec_angle = controller->inputs.a_mechAngle * controller->speed_params.poles;
    } else {
        // 此时可以安全调用calculate_hall_angle，因为已确认霍尔状态有效
        elec_angle = calculate_hall_angle(controller);
    }
    
    controller->outputs.a_elecAngle = elec_angle;
    
    // 估算转速
    uint32_t current_time = HAL_GetTick();
    controller->outputs.n_mot = estimate_speed(controller, elec_angle, current_time);
    
    // 其余代码保持不变...
    // Clarke变换（三相电流到αβ坐标系）
    int16_t i_alpha, i_beta;
    clarke_transform(controller->inputs.i_phaAB, 
                    controller->inputs.i_phaBC, 
                    -controller->inputs.i_phaAB - controller->inputs.i_phaBC,
                    &i_alpha, &i_beta);
    
    // Park变换（αβ坐标系到dq坐标系）
    int16_t i_d, i_q;
    park_transform(i_alpha, i_beta, elec_angle, &i_d, &i_q);
    
    // 根据控制模式计算dq电压
    int16_t u_d, u_q;
    
    switch (controller->inputs.z_ctrlModReq) {
        case Motor_FOC_Mode_Voltage: // 电压模式
            u_d = 0;
            u_q = controller->inputs.r_inpTgt;
            break;
            
        case Motor_FOC_Mode_Speed: // 速度模式
            {
                // 速度环PI控制
                int32_t speed_error = controller->inputs.r_inpTgt - controller->outputs.n_mot;
                int32_t i_q_ref = pi_calculate(&controller->speed_controller, speed_error);
                
                // 限制q轴电流参考值
                if (i_q_ref > controller->param.i_max) i_q_ref = controller->param.i_max;
                if (i_q_ref < -controller->param.i_max) i_q_ref = -controller->param.i_max;
                
                // 电流环PI控制
                int32_t i_q_error = i_q_ref - i_q;
                int32_t i_d_error = 0 - i_d; // d轴电流控制为0
                
                u_q = pi_calculate(&controller->iq_controller, i_q_error);
                u_d = pi_calculate(&controller->id_controller, i_d_error);
            }
            break;
            
        case Motor_FOC_Mode_Torque: // 扭矩模式
            {
                // 电流环PI控制
                int32_t i_q_error = controller->inputs.r_inpTgt - i_q;
                int32_t i_d_error = 0 - i_d; // d轴电流控制为0
                
                u_q = pi_calculate(&controller->iq_controller, i_q_error);
                u_d = pi_calculate(&controller->id_controller, i_d_error);
            }
            break;
            
        default: // 开环模式或其他
            u_d = 0;
            u_q = 0;
            break;
    }
    
    // 弱磁控制
    if (controller->param.b_fieldWeakEna) {
        // 简化的弱磁控制实现
        if (controller->outputs.n_mot > controller->param.r_fieldWeakLo) {
            int32_t fw_factor = (controller->outputs.n_mot - controller->param.r_fieldWeakLo) * 
                               controller->param.id_fieldWeakMax / 
                               (controller->param.r_fieldWeakHi - controller->param.r_fieldWeakLo);
            // 限制弱磁电流
            if (fw_factor > controller->param.id_fieldWeakMax) fw_factor = controller->param.id_fieldWeakMax;
            if (fw_factor < 0) fw_factor = 0;
            
            u_d = (int16_t)fw_factor;
        }
    }
    
    // 限制输出电压
    int32_t voltage_magnitude = (u_d * u_d + u_q * u_q) >> FIXED_SHIFT;
    int32_t max_voltage_squared = (controller->motor_param.max_voltage * controller->motor_param.max_voltage) >> FIXED_SHIFT;
    
    if (voltage_magnitude > max_voltage_squared) {
        // 按比例缩小电压矢量
        int32_t scale_factor = (max_voltage_squared << FIXED_SHIFT) / voltage_magnitude;
        u_d = (u_d * (int32_t)sqrt(scale_factor)) >> FIXED_SHIFT;
        u_q = (u_q * (int32_t)sqrt(scale_factor)) >> FIXED_SHIFT;
    }
    
    // 逆Park变换（dq坐标系到αβ坐标系）
    int16_t u_alpha, u_beta;
    inverse_park_transform(u_d, u_q, elec_angle, &u_alpha, &u_beta);
    
    // 空间矢量调制（SVM）
    space_vector_modulation(u_alpha, u_beta, 
                           &controller->outputs.DC_phaA, 
                           &controller->outputs.DC_phaB, 
                           &controller->outputs.DC_phaC);
    
    // 运行时检测
    if (FOC_RuntimeCheck(controller)) {
        controller->outputs.z_errCode = FOC_Err_Motor_Runtime_Error;
    }
    // 设置PWM输出
    motor_pwm_set_foc_outputs(g_pwm_channels, &controller->outputs);

	// 在函数结束前再次喂狗
	#ifdef USE_WATCHDOG
    HAL_IWDG_Refresh(&hiwdg);
    #endif

}

// Clarke-Park变换函数
void FOC_ClarkeParkTransform(int16_t i_alpha, int16_t i_beta, int16_t angle, int16_t* i_d, int16_t* i_q) {
    park_transform(i_alpha, i_beta, angle, i_d, i_q);
}

// 逆Clarke-Park变换函数
void FOC_InverseClarkeParkTransform(int16_t u_d, int16_t u_q, int16_t angle, int16_t* u_alpha, int16_t* u_beta) {
    inverse_park_transform(u_d, u_q, angle, u_alpha, u_beta);
}

// 空间矢量调制函数
void FOC_SpaceVectorModulation(int16_t u_alpha, int16_t u_beta, int16_t* dutyA, int16_t* dutyB, int16_t* dutyC) {
    space_vector_modulation(u_alpha, u_beta, dutyA, dutyB, dutyC);
}

// 缓启动函数
// 返回值：0=启动中，1=成功，2=失败
uint8_t FOC_SoftStart(FOC_Type* controller) {
    if (controller == NULL) {
        return 2; // 失败
    }
    
    const int16_t startupTorque = controller->param.i_max / 2;
    const uint32_t startupTimeout = 100; // 启动超时时间（毫秒）
    const uint32_t cooldownTime = 50;   // 冷却时间（毫秒）

    switch (controller->softStartStatus) {
        case SOFT_START_NOT_STARTED:
            controller->softStartStatus = SOFT_START_IN_PROGRESS;
            controller->softStartAttempts = 0;
            controller->softStartTime = HAL_GetTick();
            // 初始化启动参数
            controller->inputs.r_inpTgt = startupTorque;
            controller->inputs.z_ctrlModReq = Motor_FOC_Mode_Torque;
            break;

        case SOFT_START_IN_PROGRESS:
            // 检查是否超过最大尝试次数
            if (controller->softStartAttempts >= 5) {
                controller->softStartStatus = SOFT_START_FAILED;
                controller->outputs.z_errCode = FOC_Err_Motor_Start_Failed;
                return 2;
            }

            // 检查是否启动成功（转速大于100RPM认为成功）
            if (abs(controller->outputs.n_mot) > 100) {
                controller->softStartStatus = SOFT_START_SUCCESS;
                // 切换到速度控制模式
                controller->inputs.z_ctrlModReq = Motor_FOC_Mode_Speed;
                return 1;
            }

            // 检查是否超时
            uint32_t current_time = HAL_GetTick();
            uint32_t elapsed_time;
            if (current_time >= controller->softStartTime) {
                elapsed_time = current_time - controller->softStartTime;
            } else {
                // 处理计数器溢出
                elapsed_time = (UINT32_MAX - controller->softStartTime) + current_time + 1;
            }
            
            if (elapsed_time > startupTimeout) {
                // 超时，增加尝试次数
                controller->softStartAttempts++;
                
                // 短暂停止扭矩输出进行冷却
                controller->inputs.r_inpTgt = 0;
                controller->softStartTime = current_time + cooldownTime;
                
                // 设置冷却状态
                controller->softStartStatus = SOFT_START_COOLDOWN;
            }
            return 0;
            
        case SOFT_START_COOLDOWN:
            // 检查冷却是否完成
            current_time = HAL_GetTick();
            if (current_time >= controller->softStartTime) {
                // 冷却完成，重新开始启动尝试
                controller->softStartStatus = SOFT_START_IN_PROGRESS;
                controller->inputs.r_inpTgt = startupTorque;
                controller->softStartTime = current_time;
            }
            return 0;

        case SOFT_START_SUCCESS:
            return 1;

        case SOFT_START_FAILED:
            return 2;
            
        default:
            controller->softStartStatus = SOFT_START_NOT_STARTED;
            return 0;
    }
    
    return 0;
}

// 运行时检测函数
// 返回值：0=正常，1=异常
uint8_t FOC_RuntimeCheck(FOC_Type* controller) {
    uint8_t errorDetected = 0;
    
    // 1. 霍尔传感器检测
    uint8_t currentHallState = (controller->inputs.b_hallA << 2) | 
                               (controller->inputs.b_hallB << 1) | 
                               controller->inputs.b_hallC;
    
    // 只有当电机应该运行时才检测霍尔信号
    if (controller->softStartStatus == SOFT_START_SUCCESS && 
        abs(controller->outputs.n_mot) > 50) {
        
        // 检查霍尔信号是否有变化
        if (currentHallState == controller->lastHallState) {
            controller->hallErrorCount++;
            if (controller->hallErrorCount > 5) { // 连续5次没有变化认为错误
                errorDetected = 1;
            }
        } else {
            controller->hallErrorCount = 0;
        }
    }
    controller->lastHallState = currentHallState;
    
    // 2. 相电流检测
    int16_t i_a = controller->inputs.i_phaAB;
    int16_t i_b = controller->inputs.i_phaBC;
    int16_t i_c = -i_a - i_b; // 假设三相电流和为0
    
    // 检查各相电流是否在合理范围内
    if (abs(i_a) > controller->param.i_max || 
        abs(i_b) > controller->param.i_max || 
        abs(i_c) > controller->param.i_max) {
        controller->currentErrorCount++;
        if (controller->currentErrorCount > 3) {
            errorDetected = 1;
        }
    } else {
        controller->currentErrorCount = 0;
    }
    
    // 3. 总电流检测
    if (abs(controller->inputs.i_DCLink) > controller->param.i_max * 3 / 2) {
        errorDetected = 1;
    }
    
    return errorDetected;
}

void FOC_ResetFault(FOC_Type* controller) {
    controller->error_code = FOC_Err_None;
    controller->outputs.z_errCode = FOC_Err_None;
    for (int i=0; i<FOC_Err_MAX_ERRORS; i++) {
        controller->error_counters[i] = 0;
    }
    controller->softStartStatus = SOFT_START_NOT_STARTED; // 可能还需要复位缓启动状态
}

// 设置电机参数并重新计算控制参数
void FOC_SetMotorParameters(FOC_Type* controller, const MotorParamType* motor_params) {
    // 参数验证
    if (motor_params == NULL || 
        motor_params->phase_resistance <= 0 || 
        motor_params->phase_inductance <= 0 || 
        motor_params->back_emf_constant <= 0 ||
        motor_params->poles <= 0 ||
        motor_params->max_current <= 0 ||
        motor_params->max_voltage <= 0 ||
        motor_params->max_rpm <= 0) {
        
        if (controller != NULL) {
            controller->outputs.z_errCode = FOC_Err_Motor_Param_Invalid;
            FOC_LogEvent(FOC_LOG_EVENT_FAULT, FOC_Err_Motor_Param_Invalid);
        }
        return;
    }
	// 建议添加范围检查
	if (motor_params->phase_resistance > 10.0f || motor_params->phase_resistance < 0.01f) {
		// 电阻值不合理
		return;
	}
	if (motor_params->phase_inductance > 0.01f || motor_params->phase_inductance < 0.00001f) {
		// 电感值不合理
		return;
	}
    
	// 保存电机参数
    if (controller != NULL) {
		controller->motor_param = *motor_params;
		
		// 更新速度估算参数中的极对数
		controller->speed_params.poles = motor_params->poles;
		
		// 计算新的PI参数
		int32_t kp_current, ki_current, kp_speed, ki_speed;
		calculate_pi_params(motor_params, &kp_current, &ki_current, &kp_speed, &ki_speed);
		
		// 重新初始化PI控制器（如果已初始化）
		if (controller->id_controller.kp != 0) { // 简单检查是否已初始化
			pi_init(&controller->id_controller, 
					kp_current, 
					ki_current, 
					-controller->param.i_max, 
					controller->param.i_max);
			
			pi_init(&controller->iq_controller, 
					kp_current, 
					ki_current, 
					-controller->param.i_max, 
					controller->param.i_max);
			
			pi_init(&controller->speed_controller, 
					kp_speed, 
					ki_speed, 
					-controller->param.n_max, 
					controller->param.n_max);
		}
		
		// 重置错误状态（可选）
		controller->outputs.z_errCode = FOC_Err_None;
	}
}

// 在 FOC_UpdateInputsFromADC 函数中，确保使用DMA版本的ADC更新
void FOC_UpdateInputsFromADC(FOC_Type* controller) {
    if (controller == NULL) {
        return;
    }
    
    // 更新相电流并检查范围
    int16_t i_a = (int16_t)(motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_A) * 100);
    int16_t i_b = (int16_t)(motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_B) * 100);
    
    // 检查电流值是否在合理范围内
    if (abs(i_a) > controller->param.i_max || abs(i_b) > controller->param.i_max) {
        // 电流值异常，记录错误但不直接停止
        FOC_LogEvent(FOC_LOG_EVENT_CURRENT_WARNING, i_a > i_b ? i_a : i_b);
    }
    
    controller->inputs.i_phaAB = i_a;
    controller->inputs.i_phaBC = i_b;
    controller->inputs.i_phaAC = -(i_a + i_b);
    
    // 更新DC链路电流并应用校准
    int16_t raw_dc_current = (int16_t)(motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_DC) * 100);
    controller->inputs.i_DCLink = FOC_GetCalibratedCurrent(controller, raw_dc_current);
    
    // 更新温度数据
    controller->inputs.t_motA = (int16_t)(motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_TEMP_A) * 10);
    controller->inputs.t_motB = (int16_t)(motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_TEMP_B) * 10);
    controller->inputs.t_motC = (int16_t)(motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_TEMP_C) * 10);
    controller->inputs.t_drv = (int16_t)(motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_TEMP_DRV) * 10);
}

// 检查温度差异和驱动板温度
static uint8_t FOC_CheckTemperature(FOC_Type* controller) {
    // 检查A、B、C相温度差异
    int16_t temp_a = controller->inputs.t_motA;
    int16_t temp_b = controller->inputs.t_motB;
    int16_t temp_c = controller->inputs.t_motC;
    int16_t temp_drv = controller->inputs.t_drv;
    
    // 计算相间温度最大差异并检查是否超过限制
    {
        int16_t max_temp = temp_a > temp_b ? (temp_a > temp_c ? temp_a : temp_c) : (temp_b > temp_c ? temp_b : temp_c);
        int16_t min_temp = temp_a < temp_b ? (temp_a < temp_c ? temp_a : temp_c) : (temp_b < temp_c ? temp_b : temp_c);
        int16_t temp_variance = max_temp - min_temp;
        
        if (temp_variance > MOTOR_TEMPERATURE_VARIANCE) {
            controller->error_code = FOC_Err_Motor_OverTemperature;
            controller->outputs.z_errCode = FOC_Err_Motor_OverTemperature;
            return 1; // 温度差异过大
        }
    }
    
    // 检查驱动板温度是否超过限制
    if (temp_drv > MOTOR_DRV_TEMPERATURE_LIMIT) {
        controller->error_code = FOC_Err_Motor_OverTemperature;
        controller->outputs.z_errCode = FOC_Err_Motor_OverTemperature;
        return 1; // 驱动板温度过高
    }
    
    return 0; // 温度正常
}

// 检查驱动板电流限制
static uint8_t FOC_CheckCurrentLimit(FOC_Type* controller) {
    // 检查DC链路电流是否超过限制
    if (abs(controller->inputs.i_DCLink) > MOTOR_DRV_CURR_LIMIT) {
        controller->error_code = FOC_Err_Motor_OverCurrent;
        controller->outputs.z_errCode = FOC_Err_Motor_OverCurrent;
        return 1; // 电流超过限制
    }
    
    return 0; // 电流正常
}

// FOC输入数据验证函数
static uint8_t FOC_ValidateInputs(FOC_Type* controller) {
    // 检查控制器指针有效性
    if (controller == NULL) {
        return 0;
    }
    
    // 检查霍尔传感器输入有效性 (0或1)
    if (controller->inputs.b_hallA > 1 || 
        controller->inputs.b_hallB > 1 || 
        controller->inputs.b_hallC > 1) {
        return 0;
    }
    
    // 检查控制模式请求有效性
    if (controller->inputs.z_ctrlModReq >= Motor_FOC_Mode_Torque) { // 假设只有4种模式
        return 0;
    }
    
    // 检查电流值范围 (-FIXED_SCALE 到 FIXED_SCALE)
    if (abs(controller->inputs.i_phaAB) > FIXED_SCALE ||
        abs(controller->inputs.i_phaBC) > FIXED_SCALE ||
        abs(controller->inputs.i_phaAC) > FIXED_SCALE) {
        return 0;
    }
    
    // 检查DC链路电流范围
    if (abs(controller->inputs.i_DCLink) > MOTOR_DRV_CURR_LIMIT) {
        return 0;
    }
    
    // 检查温度值合理性 (假设温度范围在-40°C到150°C之间)
    if (controller->inputs.t_motA < -400 || controller->inputs.t_motA > 1500 ||
        controller->inputs.t_motB < -400 || controller->inputs.t_motB > 1500 ||
        controller->inputs.t_motC < -400 || controller->inputs.t_motC > 1500 ||
        controller->inputs.t_drv < -400 || controller->inputs.t_drv > 1500) {
        return 0;
    }
    
    // 检查机械角度范围 (0-16383 Q12格式)
    if (controller->inputs.a_mechAngle < 0 || controller->inputs.a_mechAngle >= 16384) {
        return 0;
    }
    
    return 1; // 所有输入验证通过
}



void FOC_LogEvent(FOC_LogEventType_e event_type, uint16_t data) {
    // 获取当前时间戳
    uint32_t timestamp = HAL_GetTick();
    
    // 循环写入日志缓冲区
    g_foc_log_buffer[g_foc_log_index].timestamp = timestamp;
    g_foc_log_buffer[g_foc_log_index].event_type = event_type;
    g_foc_log_buffer[g_foc_log_index].data = data;
    
    // 更新索引（循环缓冲区）
    g_foc_log_index = (g_foc_log_index + 1) % FOC_LOG_BUFFER_SIZE;
    
    // 可选：如果需要调试输出，可以在这里添加
    #ifdef FOC_DEBUG_LOG
    switch(event_type) {
        case FOC_LOG_EVENT_FAULT:
            printf("FOC Fault Event: %d at %lu ms\n", data, timestamp);
            break;
        case FOC_LOG_EVENT_TEMPERATURE_WARNING:
            printf("FOC Temperature Warning: %d at %lu ms\n", data, timestamp);
            break;
        case FOC_LOG_EVENT_CURRENT_WARNING:
            printf("FOC Current Warning: %d at %lu ms\n", data, timestamp);
            break;
        default:
            printf("FOC Event: %d, Data: %d at %lu ms\n", event_type, data, timestamp);
            break;
    }
    #endif
}

// 获取日志条目函数（供外部读取日志使用）
uint8_t FOC_GetLogEntry(uint16_t index, FOC_LogEntry_t* entry) {
    if (index >= FOC_LOG_BUFFER_SIZE || entry == NULL) {
        return 0;
    }
    
    *entry = g_foc_log_buffer[index];
    return 1;
}

// 清空日志缓冲区
void FOC_ClearLogBuffer(void) {
    memset(g_foc_log_buffer, 0, sizeof(g_foc_log_buffer));
    g_foc_log_index = 0;
}

// FOC复位函数
void FOC_Reset(FOC_Type* controller) {
    if (controller == NULL) {
        return;
    }
    
    // 停止PWM输出
    controller->outputs.DC_phaA = 0;
    controller->outputs.DC_phaB = 0;
    controller->outputs.DC_phaC = 0;
    motor_pwm_set_foc_outputs(g_pwm_channels, &controller->outputs);
    
    // 重置缓启动状态
    controller->softStartStatus = SOFT_START_NOT_STARTED;
    controller->softStartAttempts = 0;
    controller->softStartTime = 0;
    
    // 重置运行时检测变量
    controller->hallErrorCount = 0;
    controller->currentErrorCount = 0;
    controller->lastHallState = 0;
    
    // 重置速度估算相关变量
    controller->speed_filtered = 0;
    controller->speed_accumulator = 0;
    controller->prev_elec_angle = 0;
    controller->last_update_time = HAL_GetTick();
    controller->angle_accumulator = 0;
    controller->angle_accumulator_prev = 0;
    
    // 重置PI控制器积分项
    controller->id_controller.integral = 0;
    controller->iq_controller.integral = 0;
    controller->speed_controller.integral = 0;
    
    // 重置错误状态
    controller->error_code = FOC_Err_None;
    controller->outputs.z_errCode = FOC_Err_None;
    for (int i = 0; i < FOC_Err_MAX_ERRORS; i++) {
        controller->error_counters[i] = 0;
    }
    
    // 重置输入输出
    controller->inputs.b_motEna = 0;
    controller->inputs.z_ctrlModReq = Motor_FOC_Mode_OPEN;
    controller->inputs.r_inpTgt = 0;
    controller->outputs.n_mot = 0;
    controller->outputs.a_elecAngle = 0;
    
    // 记录复位日志
    FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0);
}
// 添加自动恢复机制
void FOC_AutoRecovery(FOC_Type* controller) {
    // 在一定条件下自动尝试恢复
    if (controller->error_code != FOC_Err_None) {
        // 尝试重置控制器
        FOC_Reset(controller);
        // 重新初始化参数
        FOC_Init(controller);
    }
}

// 增加输入数据范围检查
static uint8_t FOC_ValidateADCData(FOC_Type* controller) {
    // 检查ADC数据是否在合理范围内
    if (motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_A) > MOTOR_DRV_CURR_LIMIT ||
        motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_B) > MOTOR_DRV_CURR_LIMIT) {
        return 0; // 数据异常
    }
    return 1; // 数据正常
}

// FOC自检函数
uint8_t FOC_SelfTest(FOC_Type* controller) {
    if (controller == NULL) {
        return 0; // 自检失败
    }
    
    uint8_t test_result = 1; // 默认自检成功
    
    // 1. 检查ADC通道有效性 - 先更新ADC数据再检查 (使用DMA版本)
    motor_adc_update_all_channels_dma(); // 先读取ADC数据
    
    for (int i = 0; i < MOTOR_ADC_CHANNEL_MAX; i++) {
        // 检查ADC通道是否有效配置
        if (!g_motor_adc_channels[i].is_valid || g_motor_adc_channels[i].adc_handle == NULL) {
            controller->error_code = FOC_Err_Motor_Curr_Sensor_Failed;
            test_result = 0;
            FOC_LogEvent(FOC_LOG_EVENT_FAULT, i); // 记录具体哪个ADC通道失败
            continue;
        }
        
        // 检查ADC读取的原始值是否在合理范围内
        uint16_t raw_value = motor_adc_get_raw_value((Motor_ADC_Channel_e)i);
        if (raw_value == 0 || raw_value == 0xFFF) { // 0或最大值可能表示异常
            controller->error_code = FOC_Err_Motor_Curr_Sensor_Failed;
            test_result = 0;
            FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x1000 + i); // ADC值异常
        }
        
        // 检查转换后的值是否有效
        if (!motor_adc_is_valid((Motor_ADC_Channel_e)i)) {
            controller->error_code = FOC_Err_Motor_Curr_Sensor_Failed;
            test_result = 0;
            FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x2000 + i); // ADC转换无效
        }
    }
    
    // 2. 检查PWM通道配置
    uint8_t pwm_config_valid = 1;
    for (int i = 0; i < MOTOR_PWM_Chan_MAX; i++) {
        if (g_pwm_channels[i].htim == NULL) {
            pwm_config_valid = 0;
            FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x100 + i); // PWM通道错误
        }
    }
    if (!pwm_config_valid) {
        test_result = 0;
    }
    
    // 3. 检查霍尔传感器配置
    if (controller->hall_params.hall_order[0] == 0) {
        controller->error_code = FOC_Err_Motor_Hall_Calib_Error;
        test_result = 0;
        FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x200); // 霍尔配置错误
    }
    
    // 4. 检查电机参数有效性
    if (controller->motor_param.phase_resistance <= 0 || 
        controller->motor_param.phase_inductance <= 0 || 
        controller->motor_param.back_emf_constant <= 0 ||
        controller->motor_param.poles <= 0 ||
        controller->motor_param.max_current <= 0 ||
        controller->motor_param.max_voltage <= 0 ||
        controller->motor_param.max_rpm <= 0) {
        controller->error_code = FOC_Err_Motor_Param_Invalid;
        test_result = 0;
        FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x300); // 电机参数错误
    }
    
    // 5. 检查PI控制器参数
    if (controller->id_controller.kp == 0 && controller->iq_controller.kp == 0) {
        // PI控制器未初始化
        test_result = 0;
        FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x400); // PI控制器错误
    }
    
    // 6. 检查系统时钟
    uint32_t sys_clock = HAL_RCC_GetSysClockFreq();
    if (sys_clock == 0) {
        test_result = 0;
        FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x500); // 系统时钟错误
    }
    
    // 7. 检查关键参数范围
    if (controller->param.i_max > MOTOR_DRV_CURR_LIMIT) {
        test_result = 0;
        FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x600); // 电流限制设置错误
    }
    
    // 8. 检查温度阈值设置
    if (controller->param.temp_overtemp_threshold > 1500) { // 超过150°C
        test_result = 0;
        FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x700); // 温度阈值设置错误
    }
    
    // 9. 检查电流校准状态
    if (!controller->current_calib.is_calibrated) {
        // 尝试进行电流校准
        if (!FOC_CalibrateCurrent(controller)) {
            test_result = 0;
            FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x800); // 电流校准失败
        } else {
            FOC_LogEvent(FOC_LOG_EVENT_INIT, 0x2001); // 电流校准成功
        }
    }

    // 记录自检结果
    if (test_result) {
        FOC_LogEvent(FOC_LOG_EVENT_INIT, 0x1000); // 自检成功
    } else {
        FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x1001); // 自检失败
    }
    
    return test_result;
}

void FOC_SaveConfig(FOC_Type* controller) { 
    // 保存FOC配置到非易失性存储器（如Flash或EEPROM）
    if (controller == NULL) {
        return;
    }
    
    // 创建配置数据结构
    FOC_Config_Type config;
    config.param = controller->param;
    config.motor_param = controller->motor_param;
    config.hall_params = controller->hall_params;
    
    // 调用存储接口保存配置
    // 这里需要根据实际硬件平台实现存储功能
    // 例如: storage_save(FOC_CONFIG_ADDR, &config, sizeof(config));
    USER_CONFIG_Write((uint32_t*)&config, sizeof(config))

    FOC_LogEvent(FOC_LOG_EVENT_INIT, 0x8000); // 记录配置保存事件
}

void FOC_LoadConfig(FOC_Type* controller) { 
    // 从非易失性存储器加载FOC配置
    if (controller == NULL) {
        return;
    }
    
    // 创建配置数据结构
    FOC_Config_Type config;
    
    // 调用存储接口加载配置    
    USER_CONFIG_Read((uint32_t*)&config, sizeof(config));
    // 应用加载的配置
    controller->param = config.param;
    controller->motor_param = config.motor_param;
    controller->hall_params = config.hall_params;
    
    // 根据加载的电机参数重新计算PI控制器参数
    int32_t kp_current, ki_current, kp_speed, ki_speed;
    calculate_pi_params(&controller->motor_param, &kp_current, &ki_current, &kp_speed, &ki_speed);
    
    // 重新初始化PI控制器
    pi_init(&controller->id_controller, kp_current, ki_current, 
            -controller->param.i_max, controller->param.i_max);
    pi_init(&controller->iq_controller, kp_current, ki_current, 
            -controller->param.i_max, controller->param.i_max);
    pi_init(&controller->speed_controller, kp_speed, ki_speed, 
            -controller->param.n_max, controller->param.n_max);
    
    FOC_LogEvent(FOC_LOG_EVENT_INIT, 0x8001); // 记录配置加载事件
}

void FOC_Data_Transmit(FOC_Type* controller) {
    // 发送FOC数据（通过串口、CAN等）
    if (controller == NULL) {
        return;
    }
    
    // 创建数据结构
    FOC_Telemetry_Type telemetry;
    telemetry.speed = controller->outputs.n_mot;
    telemetry.current = controller->inputs.i_DCLink;
    telemetry.voltage = motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_DC);
    telemetry.temperature = (controller->inputs.t_motA + controller->inputs.t_motB + controller->inputs.t_motC) / 3;
    telemetry.error_code = controller->outputs.z_errCode;
    
    // 调用通信接口发送数据
    // 这里需要根据实际硬件平台实现通信功能
	#if USE_PROTOCOL_TYPE == USE_SERIAL_PORT_PROTOCOL
		uart_send(&telemetry, sizeof(telemetry));
	#endif
    
    
    FOC_LogEvent(FOC_LOG_EVENT_MODE_CHANGE, 0x8002); // 记录数据发送事件
}

void FOC_Command_Parse(FOC_Command_Type* command, uint8_t* data) {
	command->enable = data[0];
	command->mode = data[1];
	command->target = *(int16_t*)(data + 2);
}


void FOC_Data_Parse(FOC_Type* controller, uint8_t* data, uint16_t length) { 
    // 接收FOC控制命令（通过串口、CAN等）
    if (controller == NULL) {
        return;
    }

	// 创建命令数据结构
    FOC_Command_Type command;

    if(length != sizeof(FOC_Command_Type))
		return;
	else
	{
		FOC_Command_Parse(&command, data);
	}

    // 调用通信接口接收数据
    // 这里需要根据实际硬件平台实现通信功能
	#if USE_PROTOCOL_TYPE == USE_SERIAL_PORT_PROTOCOL
		uart_parse(&command, sizeof(command));
	#endif
    
    // 处理接收到的命令
    if (command.enable) {
        controller->inputs.b_motEna = 1;
        controller->inputs.z_ctrlModReq = command.mode;
        controller->inputs.r_inpTgt = command.target;
    } else {
        controller->inputs.b_motEna = 0;
    }
    
    FOC_LogEvent(FOC_LOG_EVENT_MODE_CHANGE, 0x8003); // 记录数据接收事件
}

// 设置电流校准参数
void FOC_SetCurrentCalibration(FOC_Type* controller, int16_t offset, float scale) {
    if (controller == NULL) {
        return;
    }
    
    controller->current_calib.dc_link_offset = offset;
    controller->current_calib.dc_link_scale = scale;
    controller->current_calib.is_calibrated = 1;
}

// 获取校准后的电流值
int16_t FOC_GetCalibratedCurrent(FOC_Type* controller, int16_t raw_current) {
    if (controller == NULL) {
        return 0;
    }
    
    // 如果未校准，直接返回原始值
    if (!controller->current_calib.is_calibrated) {
        return raw_current;
    }
    
    // 应用偏移量和比例因子
    int32_t calibrated_value = (int32_t)(raw_current - controller->current_calib.dc_link_offset) * 
                              controller->current_calib.dc_link_scale;
    
    return (int16_t)calibrated_value;
}

// 电流校准函数
uint8_t FOC_CalibrateCurrent(FOC_Type* controller) {
    if (controller == NULL) {
        return 0;
    }
    
    // 1. 确保电机处于停止状态
    if (controller->softStartStatus != SOFT_START_NOT_STARTED) {
        return 0; // 电机未停止，无法校准
    }
    
    // 2. 测量零电流偏移量
    int32_t offset_sum = 0;
    const uint16_t zero_sample_count = 100;
    
    for (uint16_t i = 0; i < zero_sample_count; i++) {
        // 更新ADC数据 (使用DMA版本)
        motor_adc_update_all_channels_dma();
        
        // 读取DC链路电流原始值
        int16_t raw_current = (int16_t)motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_DC);
        offset_sum += raw_current;
        
        // 增加适当的延时，确保DMA有足够时间完成下一次传输
        HAL_Delay(5); // 根据ADC采样率调整延时
    }
    
    // 计算平均偏移量
    int16_t zero_offset = (int16_t)(offset_sum / zero_sample_count);
    
    // 3. 测量不同负载下的电流值来计算比例因子
    typedef struct {
        int16_t adc_value;    // ADC读取值
        int16_t actual_current; // 实际电流值 (mA)
    } CurrentPoint;
    
    CurrentPoint calibration_points[3];
    int16_t actual_currents[3] = {5000, 10000, 12000}; // 5A, 10A, 12A
    
    // 依次设置电机到不同的功率状态并读取ADC值
    for (int i = 0; i < 3; i++) {
        // 这里需要实际设置电机负载到指定电流
        // 由于是离线校准，可能需要外部负载或已知负载特性
        // 暂时使用模拟方式读取ADC值
        
        int32_t adc_sum = 0;
        const uint16_t sample_count = 50;
        
        // 读取多个样本取平均 (使用DMA版本)
        for (uint16_t j = 0; j < sample_count; j++) {
            motor_adc_update_all_channels_dma();
            int16_t raw_current = (int16_t)motor_adc_get_converted_value(MOTOR_ADC_CHANNEL_DC);
            adc_sum += raw_current;
            // 增加适当的延时
            HAL_Delay(5); // 根据ADC采样率调整延时
        }
        
        calibration_points[i].adc_value = (int16_t)(adc_sum / sample_count) - zero_offset;
        calibration_points[i].actual_current = actual_currents[i];
    }
    
    // 4. 计算比例因子 - 使用最小二乘法计算斜率
    float sum_xy = 0.0f;
    float sum_x2 = 0.0f;
    
    for (int i = 0; i < 3; i++) {
        float x = (float)calibration_points[i].adc_value;
        float y = (float)calibration_points[i].actual_current;
        sum_xy += x * y;
        sum_x2 += x * x;
    }
    
    // 计算比例因子 (斜率)
    float scale_factor = (sum_x2 != 0.0f) ? (sum_xy / sum_x2) : 1.0f;
    
    // 5. 设置校准参数
    FOC_SetCurrentCalibration(controller, zero_offset, scale_factor);
    
    // 6. 验证校准结果
    float max_error = 0.0f;
    for (int i = 0; i < 3; i++) {
        int16_t calibrated_value = (int16_t)((float)calibration_points[i].adc_value * scale_factor);
        float error = fabsf((float)(calibrated_value - calibration_points[i].actual_current));
        if (error > max_error) {
            max_error = error;
        }
    }
    
    // 如果误差过大，校准失败
    if (max_error > 500.0f) { // 允许500mA误差
        controller->current_calib.is_calibrated = 0;
        FOC_LogEvent(FOC_LOG_EVENT_FAULT, 0x801); // 电流校准精度不足
        return 0;
    }
    
    // 7. 记录校准日志
    FOC_LogEvent(FOC_LOG_EVENT_INIT, 0x2002); // 电流校准完成
    
    return 1;
}

// 在 FOC_ClearFaults 函数中增加更多状态重置
void FOC_ClearFaults(FOC_Type* controller) {
    if (controller == NULL) {
        return;
    }
    
    // 重置所有错误状态
    controller->error_code = FOC_Err_None;
    controller->outputs.z_errCode = FOC_Err_None;
    
    // 清除所有错误计数器
    for (int i = 0; i < FOC_Err_MAX_ERRORS; i++) {
        controller->error_counters[i] = 0;
    }
    
    // 重置软启动状态
    controller->softStartStatus = SOFT_START_NOT_STARTED;
    controller->softStartAttempts = 0;
    controller->autoRecoveryAttempt = 0; // 重置自动恢复尝试次数
    
    // 重置运行时检测变量
    controller->hallErrorCount = 0;
    controller->currentErrorCount = 0;
    controller->lastHallState = 0;
    
    // 重置PI控制器积分项
    controller->id_controller.integral = 0;
    controller->iq_controller.integral = 0;
    controller->speed_controller.integral = 0;
    
    // 重置速度估算相关变量
    controller->speed_filtered = 0;
    controller->prev_elec_angle = 0;
    controller->last_update_time = HAL_GetTick();
    
    FOC_LogEvent(FOC_LOG_EVENT_INIT, 0x3000);
}

static void FOC_SetError(FOC_Type* controller, FOC_Error_Type_e error_code) {
    if (controller == NULL) return;
    
    controller->error_code = error_code;
    controller->outputs.z_errCode = error_code;
    controller->error_counters[error_code]++;
    
    FOC_LogEvent(FOC_LOG_EVENT_FAULT, error_code);
}